/*******************************************************************
* Description: genhexkins.h
*
*   Kinematics for a generalised hexapod machine
*
*   Derived from a work by R. Brian Register
*
* Adapting Author: Andrew Kyrychenko
* License: GPL Version 2
* System: Linux
*    
* Copyright (c) 2004 All rights reserved.
********************************************************************

  This is the header file to accompany genhexkins.c.  This header file
  is used to configure genhexkins.c to solve the kinematics for a default
  Stewart Platform configuration.

  */
#ifndef GENHEXKIN_H
#define GENHEXKIN_H

#include "posemath.h"
#include "genhexkins.h"

typedef struct EmcPose {
    PmCartesian tran;
    double a, b, c;
    double u, v, w;
} EmcPose;


/* the forward flags are passed to the forward kinematics so that they
   can resolve ambiguities in the world coordinates for a given joint set,
   e.g., for hexpods, this would be platform-below-base, platform-above-base.

   The flags are also passed to the inverse kinematics and are set by them,
   which is how they are changed from their initial value. For example, for
   hexapods you could do a coordinated move that brings the platform up from
   below the base to above the base. The forward flags would be set to
   indicate this. */
typedef unsigned long int KINEMATICS_FORWARD_FLAGS;

/* the inverse flags are passed to the inverse kinematics so thay they
   can resolve ambiguities in the joint angles for a given world coordinate,
   e.g., for robots, this would be elbow-up, elbow-down, etc.

   The flags are also passed to the forward kinematics and are set by them,
   which is how they are changed from their initial value. For example, for
   robots you could do a joint move that brings the elbow from a down
   configuration to an up configuration. The inverse flags would be set to
   indicate this. */
typedef unsigned long int KINEMATICS_INVERSE_FLAGS;
#define NUM_STRUTS 6 // number of struts, only 6 supported for now

struct KineConfig {
    double basex[NUM_STRUTS];
    double basey[NUM_STRUTS];
    double basez[NUM_STRUTS];
    double platformx[NUM_STRUTS];
    double platformy[NUM_STRUTS];
    double platformz[NUM_STRUTS];
    double basenx[NUM_STRUTS];
    double baseny[NUM_STRUTS];
    double basenz[NUM_STRUTS];
    double platformnx[NUM_STRUTS];
    double platformny[NUM_STRUTS];
    double platformnz[NUM_STRUTS];
    double correction[NUM_STRUTS];
    double screw_lead;
    int last_iter;
    int max_iter;
    int iter_limit;
    double max_error;
    double conv_criterion;//收敛合格标准
    double tool_offset;
    double spindle_offset;
} ;



class HexPodKinematics
{
  public:
    HexPodKinematics();

    int kinematicsForward(const double * joints,
                          EmcPose * pos,
                          const KINEMATICS_FORWARD_FLAGS * fflags,
                          KINEMATICS_INVERSE_FLAGS * iflags);
    int kinematicsInverse(const EmcPose * pos,
                          double * joints,
                          const KINEMATICS_INVERSE_FLAGS * iflags,
                          KINEMATICS_FORWARD_FLAGS * fflags);

private:

    int genhexkins_read_hal_pins(void);

    int initialKin(void);

    int StrutLengthCorrection(const PmCartesian * StrutVectUnit,
                              const PmRotationMatrix * RMatrix,
                              const int strut_number,
                              double * correction);

private:

    /* declare arrays for base and platform coordinates */
    PmCartesian b[NUM_STRUTS];
    PmCartesian a[NUM_STRUTS];

    /* declare base and platform joint axes vectors */

    PmCartesian nb1[NUM_STRUTS];
    PmCartesian na0[NUM_STRUTS];
    KineConfig *haldata;


};

#endif
